using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Diagnostics;
using System.Threading;
using System.Windows.Controls;
using System.Windows.Threading;
using CI.Organism;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.Services.IRobot.Create.Proxy;
using Microsoft.Robotics.Services.IRobot.Roomba.Proxy;
using Microsoft.Robotics.Services.IRobot.SensorUpdates.Proxy;
using W3C.Soap;
using CI;
using CI.Interfaces;
using lite = Microsoft.Robotics.Services.IRobot.Lite.Proxy;
using roomba = Microsoft.Robotics.Services.IRobot.Roomba.Proxy;


namespace iRobotController
{
    [ActivationSettings(ShareDispatcher = false, ExecutionUnitsPerDispatcher = 3)]
    [Contract(Contract.Identifier)]
    [DisplayName("iRobotController")]
    [Description("iRobotController service (Controls the Roomba)")]
    public class iRobotControllerService : DsspServiceBase, IHostingEnvironmentPhysical, INotifyPropertyChanged, IView
    {
        [ServiceState]
        iRobotControllerState _state = new iRobotControllerState();

        [ServicePort("/iRobotController", AllowMultipleInstances = false)]
        iRobotControllerOperations _mainPort = new iRobotControllerOperations();

        [Partner("bumper", Contract = lite.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting, Optional = false)]
        private readonly lite.IRobotLiteOperations _bumperPort = new lite.IRobotLiteOperations();
        private readonly lite.IRobotLiteOperations _bumperUpdates = new lite.IRobotLiteOperations();

        [Partner("roomba", Contract = roomba.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate, Optional = false)]
        private readonly RoombaOperations _roombaPort = new RoombaOperations();

        private Fault _fault;
        private bool _ready;
        private readonly SimEnv _simEnv = SimEnv.Instance;
        private IOrganism _thisHWOrganism;

        private SynapseIODelegate _leftBumperHandler;
        private SynapseIODelegate _rightBumperHandler;
        private SynapseIODelegate _cameraHandler;

        private Action<Int64> _turnLeft;
        private Action<Int64> _turnRight;
        private Action<Int64> _forward;

        private const float IRobotMaxWheelTorque = 120;
        private int _lastBump = -1;
        private Int64 _lastOutputValue;
        private string _lastBindingName;
        private readonly object _heartBeatSyncObject = new object();
        private int _heartBeatFailureCount;
        private const int HeartBeatFailureCountThreshold = 10;

        private int _bumperResult = 0;
        private int _unsafeCount = 0;
        private const int MaxUnsafeCount = 3;

        #region Roomba status properties
        private bool _rightBumperHit;
        public bool RightBumperHit
        {
            get { return _rightBumperHit; }
            set
            {
                if (_rightBumperHit == value)
                    return;

                if ((_rightBumperHandler != null) && (value))
                    _rightBumperHandler(BumperHitValue);

                _rightBumperHit = value;
                RaisePropertyChangedUIThread("RightBumperHit");
            }
        }

        private bool _leftBumperHiterHit;
        public bool LeftBumperHit
        {
            get { return _leftBumperHiterHit; }
            set
            {
                if (_leftBumperHiterHit == value)
                    return;

                if ((_leftBumperHandler != null) && (value))
                    _leftBumperHandler(BumperHitValue);

                _leftBumperHiterHit = value;
                RaisePropertyChangedUIThread("LeftBumperHit");
            }

        }

        private bool _leftCliffDetected;
        public bool LeftCliffDetected
        {
            get { return _leftCliffDetected; }
            set
            {
                if (_leftCliffDetected == value)
                    return;

                _leftCliffDetected = value;
                RaisePropertyChangedUIThread("LeftCliffDetected");
            }

        }

        private bool _rightCliffDetected;
        public bool RightCliffDetected
        {
            get { return _rightCliffDetected; }
            set
            {
                if (_rightCliffDetected == value)
                    return;

                _rightCliffDetected = value;
                RaisePropertyChangedUIThread("RightCliffDetected");
            }

        }

        private bool _leftWheelDrop;
        public bool LeftWheelDrop
        {
            get { return _leftWheelDrop; }
            set
            {
                if (_leftWheelDrop == value)
                    return;

                _leftWheelDrop = value;
                RaisePropertyChangedUIThread("LeftWheelDrop");
            }

        }

        private bool _ignoreLeftWheelDrop;
        public bool IgnoreLeftWheelDrop
        {
            get { return _ignoreLeftWheelDrop; }
            set
            {
                if (_ignoreLeftWheelDrop == value)
                    return;

                _ignoreLeftWheelDrop = value;
                RaisePropertyChanged("IgnoreLeftWheelDrop");
            }
        }

        private bool _rightWheelDrop;
        public bool RightWheelDrop
        {
            get { return _rightWheelDrop; }
            set
            {
                if (_rightWheelDrop == value)
                    return;

                _rightWheelDrop = value;
                RaisePropertyChangedUIThread("RightWheelDrop");
            }

        }

        private bool _ignoreRightWheelDrop;
        public bool IgnoreRightWheelDrop
        {
            get { return _ignoreRightWheelDrop; }
            set
            {
                if (_ignoreRightWheelDrop == value)
                    return;

                _ignoreRightWheelDrop = value;
                RaisePropertyChanged("IgnoreRightWheelDrop");
            }
        }

        private bool _leftWheelOvercurrent;
        public bool LeftWheelOvercurrent
        {
            get { return _leftWheelOvercurrent; }
            set
            {
                if (_leftWheelOvercurrent == value)
                    return;

                _leftWheelOvercurrent = value;
                RaisePropertyChangedUIThread("LeftWheelOvercurrent");
            }

        }

        private bool _rightWheelOvercurrent;
        public bool RightWheelOvercurrent
        {
            get { return _rightWheelOvercurrent; }
            set
            {
                if (_rightWheelOvercurrent == value)
                    return;

                _rightWheelOvercurrent = value;
                RaisePropertyChangedUIThread("RightWheelOvercurrent");
            }

        }

        private bool _sideBrushOvercurrent;
        public bool SideBrushOvercurrent
        {
            get { return _sideBrushOvercurrent; }
            set
            {
                if (_sideBrushOvercurrent == value)
                    return;

                _sideBrushOvercurrent = value;
                RaisePropertyChangedUIThread("SideBrushOvercurrent");
            }

        }

        private bool _vacuumOvercurrent;
        public bool VacuumOvercurrent
        {
            get { return _vacuumOvercurrent; }
            set
            {
                if (_vacuumOvercurrent == value)
                    return;

                _vacuumOvercurrent = value;
                RaisePropertyChangedUIThread("VacuumOvercurrent");
            }

        }

        private bool _mainBrushOvercurrent;
        public bool MainBrushOvercurrent
        {
            get { return _mainBrushOvercurrent; }
            set
            {
                if (_mainBrushOvercurrent == value)
                    return;

                _mainBrushOvercurrent = value;
                RaisePropertyChangedUIThread("MainBrushOvercurrent");
            }

        }

        private bool _ignoreMainBrushOverCurrent;
        public bool IgnoreMainBrushOverCurrent
        {
            get { return _ignoreMainBrushOverCurrent; }
            set
            {
                if (_ignoreMainBrushOverCurrent == value)
                    return;

                _ignoreMainBrushOverCurrent = value;
                RaisePropertyChanged("IgnoreMainBrushOverCurrent");
            }
        }

        private decimal _chargeStatePercentage;
        public decimal ChargeStatePercentage
        {
            get { return _chargeStatePercentage; }
            set
            {
                if (_chargeStatePercentage == value)
                    return;

                _chargeStatePercentage = value;
                RaisePropertyChanged("ChargeStatePercentage");
            }
        }

        private string _chargeState;
        public string ChargeState
        {
            get { return _chargeState; }
            set
            {

                if (value == "Waiting")
                    value = "Not charging";

                if (_chargeState == value)
                    return;

                _chargeState = value;
                RaisePropertyChanged("ChargeState");
            }
        }

        private bool _heatBeatFire;
        public bool HeartBeatFire
        {
            get { return _heatBeatFire; }
            set
            {
                _simEnv.TraceLogger.LogDebug("Heartbeat fired : " + value);

                if (_heatBeatFire == value)
                    return;

                _heatBeatFire = value;
                RaisePropertyChanged("HeartBeatFire");
            }
        }

        private int _heartBeatCheckTime = 500;
        public int HeartBeatCheckTime
        {
            get { return _heartBeatCheckTime; }
            set
            {
                if (_heartBeatCheckTime == value)
                    return;

                _heartBeatCheckTime = value;
                StartHeartBeatTimer(_heartBeatCheckTime);
                RaisePropertyChanged("HeartBeatCheckTime");
                Trace.WriteLine("Setting New HB Check Time " + _heartBeatCheckTime);
            }
        }

        #endregion

        private const int BumperHitValue = 100;
        private bool _isRoombaCharging;
        private bool _batteryLow;
        private bool _vacuumLastStateOn;

        private readonly TimeSpan _hwDriveFaultTolerenceTime = new TimeSpan(0, 0, 0, 0, 100);
        private readonly TimeSpan _hwDriveFaultTolerenceResetTime = new TimeSpan(0, 0, 0, 1, 0);
        private readonly TimeSpan _hwvacuumFaultTolerenceTime = new TimeSpan(0, 0, 0, 0, 250);
        private readonly TimeSpan _hwvacuumFaultTolerenceResetTime = new TimeSpan(0, 0, 0, 0, 500);
        private HWFaultTolerence _hwDriveFaultTolerence;
        private HWFaultTolerence _hwvacuumFaultTolerence;
        private const decimal ChargeStateThreshold = 50;

        public event PropertyChangedEventHandler PropertyChanged;
        private System.Windows.Threading.Dispatcher _uiDispatcher;

        // note only to be set when you dont'have real hw 
        private bool TESTING_WITH_NO_HW = false;

        // to be used with hw
        private bool TESTING_QUIET_NO_DRIVE = false;
        private bool TESTING_QUIET_NO_VACUUM = false;

        private bool _shuttingdown = false;

        private UserControl _displayControl;
        public UserControl DisplayControl
        {
            get
            {
                if (_displayControl == null)
                {
                    _uiDispatcher = System.Windows.Threading.Dispatcher.CurrentDispatcher;
                    _displayControl = new RoombaStatus(this);
                }

                return _displayControl;
            }
        }

        private readonly Action<string> _raiseUINotify;
        private readonly TimeSpan _heartBeatFailureSleepTime = new TimeSpan(0, 0, 0, 1);
        private bool _attemptingRestart;
        private Timer _heartBeatTimer;
        private Timer _waitAndRetryTimer;
        private Timer _bumperCheckerTimer;
        private readonly TimeSpan _recoveryWaitTime = new TimeSpan(0, 0, 5);
        private const int LastProblemLeewayTime = 2;
        private string _lastProblemReason;
        private DateTime _lastProblemTime = DateTime.Now;
        private readonly TimeSpan _roombaStopSleep = new TimeSpan(0, 0, 0, 1);
        private readonly TimeSpan _emergencyReverseTime = new TimeSpan(0, 0, 0, 0, 500);
        private DateTime _lastEmergencyReverseTime = DateTime.Now;
        private bool _stopping;


        public iRobotControllerService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
            _simEnv.UsingHW = true;
            _raiseUINotify = new Action<string>(RaisePropertyChanged);
        }

        protected override void Start()
        {
            base.Start();

            _turnLeft = new Action<long>(TurnLeft);
            _turnRight = new Action<long>(TurnRight);
            _forward = new Action<long>(Forward);

            _simEnv.TraceLogger.LogDebug("Starting iRobot robotics engine");

            // save hosting environment
            _simEnv.HostEnvPhysical = this;
            _simEnv.OnPauseEvent += Pause;

            // set fault detecting
            _hwDriveFaultTolerence = new HWFaultTolerence(_hwDriveFaultTolerenceTime, _hwDriveFaultTolerenceResetTime);
            _hwvacuumFaultTolerence = new HWFaultTolerence(_hwvacuumFaultTolerenceTime, _hwvacuumFaultTolerenceResetTime);

            // Subscribe to the iRobot sensors
            _bumperPort.Subscribe(_bumperUpdates);

            SpawnIterator(StartRoomba);
        }

        private void UpdateBumpsHandler(UpdateBumpsCliffsAndWalls update)
        {
            HeartBeatFire = true;

            CheckForRestarting();

            lock (this)
            {
                // if we have an issue 
                IsRoombaSafe(update);
                CheckAndUpdateBumpers(update);
                CheckIsSafeCliff(update);
                CheckIsSafeWheelsDown(update);
                CheckIsSafeVirtualWall(update);
            }
        }

        private void UpdatePowerHandler(UpdatePower updatePower)
        {

            HeartBeatFire = true;

            CheckForRestarting();

            if (!IsRoombaBatteryWorking(updatePower))
            {
                _simEnv.SetPause(true);
                const string state = "Battery Fault, try recharging";
                _simEnv.HostEnv.ProcessingState = state;
                _batteryLow = true;
                return;
            }


            decimal chargeStatePercentage = ((decimal)updatePower.Body.Charge / (decimal)updatePower.Body.Capacity) * 100;

            ChargeState = updatePower.Body.ChargingState.ToString();

            ChargeStatePercentage = (int)chargeStatePercentage;

            _isRoombaCharging = updatePower.Body.ChargingState == ChargingState.Charging;
            _simEnv.TraceLogger.LogDebug("Charge state : " + chargeStatePercentage);
            _simEnv.TraceLogger.LogDebug("Charge state : " + updatePower.Body.ChargingState);

            if (_simEnv.PauseState)
                return;


            if (chargeStatePercentage < ChargeStateThreshold)
            {
                _simEnv.SetPause(true);
                _batteryLow = true;
                SpawnIterator(SeekDock);
            }
            else
            {
                _batteryLow = false;
            }


        }

        private static bool IsRoombaBatteryWorking(UpdatePower updatePower)
        {
            if (updatePower.Body.Capacity == 0)
                return false;

            if (updatePower.Body.Charge == 0)
                return false;

            if (updatePower.Body.Charge > updatePower.Body.Capacity)
                return false;

            return true;
        }

        private IEnumerator<ITask> SeekDock()
        {
            const string state = "Low batterys, searching for charger";
            _simEnv.HostEnv.ProcessingState = state;
            _simEnv.TraceLogger.LogDebug(state);
            _roombaPort.RoombaSetMode(RoombaMode.Passive, true);
            Thread.Sleep(2000);
            _roombaPort.RoombaSeekDock();
            Thread.Sleep(2000);

            return null;
        }

        private void IsRoombaSafe(UpdateBumpsCliffsAndWalls update)
        {
            if (update.Body.MotorOvercurrents == 0)
            {
                LeftWheelOvercurrent = false;
                RightWheelOvercurrent = false;
                VacuumOvercurrent = false;
                SideBrushOvercurrent = false;
                MainBrushOvercurrent = false;
                return;
            }

            _simEnv.TraceLogger.LogDebug("State update : " + update.Body.MotorOvercurrents);

            var now = DateTime.Now;

            CheckIsSafeDriveWheelsOverCurrent(update, now);
            CheckIsSafeVacuumingOverCurrent(update, now);
        }

        private void CheckIsSafeVirtualWall(UpdateBumpsCliffsAndWalls update)
        {
            if (update.Body.VirtualWall)
                SetRoombaIsNotSafe("Virtual Wall detected", false);
        }

        private void CheckIsSafeWheelsDown(UpdateBumpsCliffsAndWalls update)
        {
            if ((update.Body.BumpsWheeldrops & BumpsWheeldrops.WheelDropRight) == BumpsWheeldrops.WheelDropRight)
            {
                RightWheelDrop = true;
                if (!IgnoreRightWheelDrop)
                    SetRoombaIsNotSafe("Right Wheel is off the ground", true);
            }
            else
            {
                RightWheelDrop = false;
            }

            if ((update.Body.BumpsWheeldrops & BumpsWheeldrops.WheelDropLeft) == BumpsWheeldrops.WheelDropLeft)
            {
                LeftWheelDrop = true;
                if (!IgnoreLeftWheelDrop)
                    SetRoombaIsNotSafe("Left Wheel is off the ground", true);
            }
            else
            {
                LeftWheelDrop = false;
            }

            if ((update.Body.BumpsWheeldrops & BumpsWheeldrops.WheelDropCaster) == BumpsWheeldrops.WheelDropCaster)
                SetRoombaIsNotSafe("Front Wheel is off the ground", true);
        }

        private void CheckIsSafeCliff(UpdateBumpsCliffsAndWalls update)
        {
            if (update.Body.CliffFrontLeft || update.Body.CliffLeft)
            {
                LeftCliffDetected = true;
                SetRoombaIsNotSafe("Left Cliff detected", false);
            }
            else
            {
                LeftCliffDetected = false;
            }

            if (update.Body.CliffFrontRight || update.Body.CliffRight)
            {
                RightCliffDetected = true;
                SetRoombaIsNotSafe("Right Cliff detected", false);
            }
            else
            {
                RightCliffDetected = false;
            }

        }

        private void CheckIsSafeVacuumingOverCurrent(UpdateBumpsCliffsAndWalls update, DateTime now)
        {

            if ((update.Body.MotorOvercurrents & MotorOvercurrents.MainBrush) == MotorOvercurrents.MainBrush)
            {
                MainBrushOvercurrent = true;
                if (!IgnoreMainBrushOverCurrent)
                    if (_hwDriveFaultTolerence.HasFaultTolerenceThresholdBeenReached(now))
                        SetRoombaIsNotSafe(update.Body.MotorOvercurrents.ToString(), true);
            }
            else
            {
                MainBrushOvercurrent = false;
            }

            if ((update.Body.MotorOvercurrents & MotorOvercurrents.SideBrush) == MotorOvercurrents.SideBrush)
            {
                SideBrushOvercurrent = true;
                if (_hwDriveFaultTolerence.HasFaultTolerenceThresholdBeenReached(now))
                    SetRoombaIsNotSafe(update.Body.MotorOvercurrents.ToString(), true);
            }
            else
            {
                SideBrushOvercurrent = false;
            }

            if ((update.Body.MotorOvercurrents & MotorOvercurrents.Vacuum) == MotorOvercurrents.Vacuum)
            {
                VacuumOvercurrent = true;
                if (_hwDriveFaultTolerence.HasFaultTolerenceThresholdBeenReached(now))
                    SetRoombaIsNotSafe(update.Body.MotorOvercurrents.ToString(), true);
            }
            else
            {
                VacuumOvercurrent = false;
            }
        }

        private void CheckIsSafeDriveWheelsOverCurrent(UpdateBumpsCliffsAndWalls update, DateTime now)
        {
            _simEnv.TraceLogger.LogDebug(update.Body.MotorOvercurrents + ":" + now.TimeOfDay);
            if ((update.Body.MotorOvercurrents & MotorOvercurrents.DriveLeft) == MotorOvercurrents.DriveLeft)
            {
                LeftWheelOvercurrent = true;
                if (_hwvacuumFaultTolerence.HasFaultTolerenceThresholdBeenReached(now))
                    SetRoombaIsNotSafe(update.Body.MotorOvercurrents.ToString(), true);
            }
            else
            {
                LeftWheelOvercurrent = false;
            }

            if ((update.Body.MotorOvercurrents & MotorOvercurrents.DriveRight) == MotorOvercurrents.DriveRight)
            {
                RightWheelOvercurrent = true;
                if (_hwvacuumFaultTolerence.HasFaultTolerenceThresholdBeenReached(now))
                    SetRoombaIsNotSafe(update.Body.MotorOvercurrents.ToString(), true);
            }
            else
            {
                RightWheelOvercurrent = false;
            }
        }

        private void SetRoombaIsNotSafe(string problemPart, bool waitAndRetry)
        {
            var overcurrentState = "Check : " + problemPart;
            _simEnv.TraceLogger.LogWarn("Roomba is not Safe " + overcurrentState);

            // first thing, pause it
            _simEnv.SetPause(true);

            // show updated state
            _simEnv.HostEnv.ProcessingState = overcurrentState;

            // now try and stop engine and vacuum
            RoombaDriveStop();
            SetVacumStateOn(false);

            EmergencyReverse();

            if (!waitAndRetry)
                return;

            if ((problemPart != _lastProblemReason)
                || (_lastProblemTime.Add(_recoveryWaitTime).AddSeconds(LastProblemLeewayTime) < DateTime.Now))
            {

                _unsafeCount++;
                if (_unsafeCount >= MaxUnsafeCount)
                {
                    _unsafeCount = 0;
                    const string state = "Restarting scenario with new NN, too many failures with this one";
                    _simEnv.TraceLogger.LogDebug(state);
                    _simEnv.TriggerScenarioRestart(state);
                    return;
                }

                _lastProblemReason = problemPart;
                _lastProblemTime = DateTime.Now;
                _simEnv.TraceLogger.LogWarn("New proplem acknowleged");
                _waitAndRetryTimer = new Timer(UnPauseAndAttemptRecovery, null, _recoveryWaitTime, new TimeSpan(-1));
            }
        }

        private void UnPauseAndAttemptRecovery(object state)
        {
            _simEnv.SetPause(false);

            // show updated state
            _simEnv.HostEnv.ProcessingState = "Attempting recovery from : " + _lastProblemReason;

            if (_waitAndRetryTimer != null)
                _waitAndRetryTimer.Dispose();
        }

        private void CheckAndUpdateBumpers(UpdateBumpsCliffsAndWalls update)
        {

            if ((int)update.Body.BumpsWheeldrops == _lastBump)
            {
                _lastBump = -1;
                return;
            }

            _lastBump = (int)update.Body.BumpsWheeldrops;

            _simEnv.TraceLogger.LogDebug("Bump wheel drops state :" + update.Body.BumpsWheeldrops);
            RightBumperHit = (update.Body.BumpsWheeldrops & BumpsWheeldrops.BumpRight) == BumpsWheeldrops.BumpRight;
            LeftBumperHit = (update.Body.BumpsWheeldrops & BumpsWheeldrops.BumpLeft) == BumpsWheeldrops.BumpLeft;
        }

        private IEnumerator<ITask> StartRoomba()
        {
            _simEnv.TraceLogger.LogDebug("Starting Roomba");

            yield return Arbiter.Choice(_bumperPort.Subscribe(_bumperUpdates),
                StartupSubscribeResponseSuccess,
                StartupSubscribeResponseFailure);

            if (_fault != null)
                yield break;

            int tries = 0;
            _ready = false;
            while (!_ready && tries < 60)
            {
                yield return Arbiter.Choice(
                    Arbiter.Receive<UpdateMode>(false, _bumperUpdates, OnRoombaConnected),
                    Arbiter.Receive(false, TimeoutPort(1000), StartupSensorWaitNotify));

                tries++;
            }

            if (!_ready)
            {
                const string message = "ABORTING: Roomba failed to initialize.";
                _simEnv.TraceLogger.LogWarn(message);
                _simEnv.HostEnv.ProcessingState = message;
                yield break;
            }
        }

        private void StartupSubscribeResponseSuccess(SubscribeResponseType response)
        {
            _ready = true;
        }



        private void CheckBumperState(object state)
        {
            if (_bumperResult > 1)
                return;

            Interlocked.Increment(ref _bumperResult);
            SpawnIterator(CheckBumper);
        }

        private void StartupSubscribeResponseFailure(Fault f)
        {
            _fault = f;
            var faultText = (_fault.Reason == null || _fault.Reason.Length < 1) ? "Failure subscribing to iRobot Roomba" : _fault.Reason[0].Value;
            _simEnv.TraceLogger.LogException("Exception starting roomba", new Exception(faultText));
        }

        private void OnRoombaConnected(UpdateMode notification)
        {
            if (notification.Body.RoombaMode == RoombaMode.Passive
                || notification.Body.RoombaMode == RoombaMode.Safe
                || notification.Body.RoombaMode == RoombaMode.Full
                )
            {
                _simEnv.TraceLogger.LogDebug("Roomba started mode : " + notification.Body.RoombaMode);
                _ready = true;

                Activate<ITask>(Arbiter.Receive<UpdateBumpsCliffsAndWalls>(true, _bumperUpdates, UpdateBumpsHandler));
                Activate<ITask>(Arbiter.Receive<UpdatePower>(true, _bumperUpdates, UpdatePowerHandler));
                Activate<ITask>(Arbiter.Receive<UpdateMode>(true, _bumperUpdates, UpdateModeHandler));
                Activate<ITask>(Arbiter.Receive<UpdatePose>(true, _bumperUpdates, UpdatePoseHandler));

                // setup timer to check on heartbeat
                StartHeartBeatTimer(HeartBeatCheckTime);
                _roombaPort.RoombaPlaySong(1);
                SetVacumStateOn(true);
                _roombaPort.RoombaPlaySong(2);
                RobotInitCompleted();
            }
            else
                _simEnv.TraceLogger.LogDebug("Waiting for Roomba to be initialized (" + notification.Body.RoombaMode + ")");
        }

        private void StartHeartBeatTimer(int checkTime)
        {
            if (_heartBeatTimer != null)
                _heartBeatTimer.Dispose();

            _heartBeatTimer = new Timer(HeartBeatTimer, null, checkTime, System.Threading.Timeout.Infinite);
        }

        private void UpdatePoseHandler(UpdatePose pose)
        {
            HeartBeatFire = true;
        }

        private void HeartBeatTimer(object timerState)
        {
            if (Monitor.TryEnter(_heartBeatSyncObject))
            {
                try
                {
                    _simEnv.TraceLogger.LogDebug("Firing heartbeat timer - HeartBeatFire : " + HeartBeatFire);

                    if (_attemptingRestart)
                        return;

                    if (_shuttingdown)
                        return;

                    if (!HeartBeatFire && !_simEnv.PauseState)
                    {
                        var state = "Lost connection with  roomba, Attempting to send stop command...";
                        _simEnv.TraceLogger.LogDebug(state);

                        if (_simEnv.HostEnv != null)
                            _simEnv.HostEnv.ProcessingState = state;

                        _simEnv.SetPause(true);

                        _heartBeatFailureCount = 1;

                        do
                        {
                            RoombaDriveStop();

                            Thread.Sleep(_heartBeatFailureSleepTime);

                            if (_heartBeatFailureCount > HeartBeatFailureCountThreshold)
                            {
                                _attemptingRestart = true;
                                _heartBeatFailureCount = 0;
                                _simEnv.SetPause(false);
                                state = "Attempting restart of robotic service to reconnect to roomba";
                                _simEnv.TriggerServiceAndScenarioRestart(state);
                                _simEnv.TraceLogger.LogDebug(state);
                                _simEnv.HostEnv.ProcessingState = state;

                                return;
                            }

                            _heartBeatFailureCount++;

                        } while (!HeartBeatFire);

                        _simEnv.SetPause(false);
                        state = "Connection with  roomba restored";
                        _simEnv.TraceLogger.LogDebug(state);
                        _simEnv.HostEnv.ProcessingState = state;
                    }

                }
                finally
                {
                    _heartBeatFailureCount = 0;
                    HeartBeatFire = false;
                    StartHeartBeatTimer(HeartBeatCheckTime);
                    Monitor.Exit(_heartBeatSyncObject);
                }


            }

            // i think this is because we dont want the timer rentering and changing the heartbeat count
            //else if (_heartBeatFailureCount == 0)
            //    HeartBeatFire = false;
        }

        private void UpdateModeHandler(UpdateMode mode)
        {
            CheckForRestarting();

            HeartBeatFire = true;
            _vacuumLastStateOn = false;
            _simEnv.TraceLogger.LogDebug("Roomba switched to  mode : " + mode.Body.RoombaMode);

            // when roomba switches to safe then back to full again it shuts down the vacuum
            if (!_isRoombaCharging || !_batteryLow)
            {
                SetVacumStateOn(true);
            }
        }

        private void CheckForRestarting()
        {
            lock (this)
            {
                if (_attemptingRestart)
                {
                    _attemptingRestart = false;
                    _simEnv.TraceLogger.LogDebug("Restart of service detected");
                    RobotInitCompleted();
                    _simEnv.SetPause(false);
                }
            }
        }

        private void SetVacumStateOn(bool on)
        {
            if (_isRoombaCharging || _batteryLow || _vacuumLastStateOn == on)
                return;

            _vacuumLastStateOn = on;

            _simEnv.TraceLogger.LogDebug("Setting vacuum state to on:" + on);

            if (TESTING_QUIET_NO_VACUUM)
                return;

            if (on)
                _roombaPort.RoombaSetCleaningMotors(RoombaMotorBits.MainBrush | RoombaMotorBits.Vacuum | RoombaMotorBits.SideBrush);
            else
                _roombaPort.RoombaSetCleaningMotors(new CmdMotors());
        }

        private void RobotInitCompleted()
        {
            if (!TESTING_WITH_NO_HW)
                _bumperCheckerTimer = new Timer(CheckBumperState, null, 1000, 50);

            ReleaseSemaphore();
        }

        private static void ReleaseSemaphore()
        {
            Semaphore.OpenExisting("MSRB").Release(1);
        }

        private void StartupSensorWaitNotify(DateTime timeout)
        {
            _simEnv.TraceLogger.LogDebug("Waiting for Roomba to be initialized...");

            // todo temp while developing away from real hw
            if (TESTING_WITH_NO_HW)
            {
                RobotInitCompleted();
                _ready = true;
            }
        }

        public ILocation GetLocation(IOrganism organism)
        {
            throw new NotImplementedException();
        }

        public ILocation SetLocation(IOrganism organism, ILocation location)
        {
            throw new NotImplementedException();
        }

        public bool RegisterCell(ICell cell, ActionType actionType)
        {
            if (actionType == ActionType.AddCell)
            {
                _thisHWOrganism = cell.Organism;
                _thisHWOrganism.NeuralSystem.NotifyEvaluationStart += NeuralSystem_NotifyEvaluationStart;
                SetVacumStateOn(true);
                return true;
            }

            return false;
        }

        private void NeuralSystem_NotifyEvaluationStart()
        {
            _simEnv.TraceLogger.LogDebug("NeuralSystem_NotifyEvaluationStart");

            if ((RightBumperHit) && (_rightBumperHandler != null))
                _rightBumperHandler(BumperHitValue);
            else if ((LeftBumperHit) && (_leftBumperHandler != null))
                _leftBumperHandler(BumperHitValue);
        }

        private IEnumerator<ITask> CheckBumper()
        {
            var bumperRequest = new CmdSensors(CreateSensorPacket.AllBumpsCliffsAndWalls);
            var responsePort = _roombaPort.RoombaGetSensors(bumperRequest);

            yield return Arbiter.Choice(
                    Arbiter.Receive<ReturnSensors>(false, responsePort, SensorHandlerResult),
                    Arbiter.Receive<Fault>(false, responsePort, SensorHandlerFault));

        }

        private void SensorHandlerResult(ReturnSensors receivedStart)
        {
            UpdateBumpsHandler(new UpdateBumpsCliffsAndWalls(receivedStart));
            Interlocked.Decrement(ref _bumperResult);
        }

        private void SensorHandlerFault(Fault fault)
        {
            _simEnv.TraceLogger.LogWarn("Failed to Received sensor information");
            Interlocked.Decrement(ref _bumperResult);
        }

        public bool Registered(IOrganism organism)
        {
            return organism == _thisHWOrganism;
        }

        private void RaisePropertyChangedUIThread(string propertyName)
        {
            if (_uiDispatcher != null)
                _uiDispatcher.BeginInvoke(DispatcherPriority.Render, _raiseUINotify, propertyName);
        }

        private void RaisePropertyChanged(string propertyName)
        {
            if ((propertyName != null) && (PropertyChanged != null))
            {
                PropertyChanged(this, new PropertyChangedEventArgs(propertyName));
            }
        }



        /// <summary>
        /// Bind a name of a IO Event in the physical world to a synapse
        /// eg "Left Bumper" to a IO synapse, "Right Drive" to a IO synapse
        /// </summary>
        /// <param name="organism">Organism its being bound to</param>
        public void BindSynapseIO(IOrganism organism)
        {
            iRobot.BindSynapseIO(organism, ref _leftBumperHandler, ref _rightBumperHandler,ref _cameraHandler, ref _turnLeft, ref _turnRight, ref _forward);
        }

        /// <summary>
        /// UnBind a name of a IO Event in the physical world to a synapse
        /// eg "Left Bumper" to a IO synapse, "Right Drive" to a IO synapse
        /// </summary>
        /// <param name="organism">Organism its being unbound from</param>
        public void UnBindSynapseIO(IOrganism organism)
        {
            iRobot.UnbindSynapseIO(organism, ref _leftBumperHandler, ref _rightBumperHandler,ref _cameraHandler, ref _turnLeft, ref _turnRight, ref _forward);
        }

        private void TurnLeft(Int64 outputValue)
        {
            OutputBoundServices(outputValue, Globals.IROBOTTURNLEFT);
        }

        private void TurnRight(Int64 outputValue)
        {
            OutputBoundServices(outputValue, Globals.IROBOTTURNRIGHT);
        }

        private void Forward(Int64 outputValue)
        {
            OutputBoundServices(outputValue, Globals.IROBOTFORWARD);
        }

        public void CleanUp()
        {
            _simEnv.TraceLogger.LogDebug("Cleaning up/Shutting down System");
            RoombaDriveStop();
            SetVacumStateOn(false);
            _thisHWOrganism.NeuralSystem.NotifyEvaluationStart -= NeuralSystem_NotifyEvaluationStart;
        }

        private void EmergencyReverse()
        {
            if (_lastEmergencyReverseTime.Add(_emergencyReverseTime).AddSeconds(LastProblemLeewayTime) < DateTime.Now)
            {
                _lastEmergencyReverseTime = DateTime.Now;
                _simEnv.TraceLogger.LogWarn("EmergencyReverse");
                _roombaPort.RoombaDrive(-100, 0);
                Thread.Sleep(_emergencyReverseTime);
                RoombaDriveStop();
            }
        }

        private void RoombaDriveStop()
        {
            // spawn stop on update action
            if (_stopping)
                return;

            if (_isRoombaCharging || _batteryLow)
                return;

            _simEnv.TraceLogger.LogDebug("Firing Roomba Drive SpawnedStop");

            _stopping = true;

            SpawnIterator(SpawnedStop);
            _lastOutputValue = 0;
            _lastBindingName = null;
        }

        private IEnumerator<ITask> SpawnedStop()
        {
            _simEnv.TraceLogger.LogDebug("Firing Roomba Stop 1st");
            _roombaPort.RoombaDrive(0, 0);
            Thread.Sleep(_roombaStopSleep);
            _simEnv.TraceLogger.LogDebug("Firing Roomba Stop 2nd");
            _roombaPort.RoombaDrive(0, 0);
            _stopping = false;
            yield break;
        }

        public void WaitForIdle()
        {
            // reset his energy so he lasts
            if (_thisHWOrganism != null)
                _thisHWOrganism.Energy = _thisHWOrganism.InitEnergy;
        }

        public void ShutDownService()
        {
            if (_heartBeatTimer != null)
                _heartBeatTimer.Dispose();

            if (_waitAndRetryTimer != null)
                _waitAndRetryTimer.Dispose();

            if (_bumperCheckerTimer != null)
                _bumperCheckerTimer.Dispose();

            _shuttingdown = true;
            Break();
        }

        public void SanityCheck()
        {

        }

        public void Break()
        {
            RoombaDriveStop();
            SetVacumStateOn(false);
        }

        public int MaxEntitysSupported
        {
            get { return 1; }
        }

        public int EntitysConsumed
        {
            get
            {
                if (_thisHWOrganism == null)
                    return 0;

                return 1;
            }
        }

        public void HideExternalCameraWindow(bool hide)
        { }

        /// <summary>
        /// Process all the output services, check their name in the binding name then call their function
        /// </summary>
        /// <param name="outputValue">output value to use</param>
        /// <param name="bindingName">binding name</param>
        private void OutputBoundServices(Int64 outputValue, string bindingName)
        {
            // if we are testing
            if (NotAllowedToDrive())
                return;

            // if it is above the max then use the max
            if (outputValue > IRobotMaxWheelTorque)
                outputValue = (long)IRobotMaxWheelTorque;

            // if still doing the same thing no need to resend command
            // todo : add a checking of 10%, so if the change is within 10% ignore it
            if (bindingName == _lastBindingName && outputValue == _lastOutputValue)
                return;

            _lastBindingName = bindingName;
            _lastOutputValue = outputValue;

            _simEnv.TraceLogger.LogDebug(string.Format("Executing on output value of {0} service {1}", outputValue, bindingName));



            switch (bindingName)
            {
                case Globals.IROBOTFORWARD:
                    _roombaPort.RoombaDrive((int)outputValue, 0);
                    break;

                case Globals.IROBOTTURNLEFT:
                    _roombaPort.RoombaDrive((int)outputValue, 1);
                    break;

                case Globals.IROBOTTURNRIGHT:
                    _roombaPort.RoombaDrive((int)outputValue, -1);
                    break;
            }

        }

        private bool NotAllowedToDrive()
        {
            if (TESTING_QUIET_NO_DRIVE)
                return true;

            // if we have paused
            if (_simEnv.PauseState)
                return true;

            // if we havent recieved a heartbeat in a while dont drive
            if (!HeartBeatFire)
                return true;

            // if we are charging or low battery don't drive
            if (_isRoombaCharging || _batteryLow)
                return true;

            return false;
        }

        /// <summary>
        /// Pause the environment
        /// </summary>
        private void Pause(bool paused)
        {
            if (paused)
                RoombaDriveStop();

            SetVacumStateOn(!paused);
            _simEnv.TraceLogger.LogDebug("Pause state :" + paused);
        }

        public void Dispose()
        { }
    }
}


